//
// Created by 16933 on 2022/5/2.
//

#include "Control.h"
#include "cmsis_os.h"
#include "tim.h"
#include "bsp_Motor.h"

extern motor_measure_t Motor_Left;   //左边电机
extern motor_measure_t Motor_Right;  //右边电机
extern float PID_Speed_Lout;
extern float PID_Speed_Rout;
extern int speed;
void ControlTask(void const * argument)
{
    HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_1);
    HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_2);
    HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_3);
    HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_4);
    uint32_t Contiol_time = osKernelSysTick();
    for(;;)
    {
//        Motor_Left_Move(PID_Speed_Lout);
//        HAL_GPIO_WritePin(TEST_GPIO_Port,TEST_Pin,GPIO_PIN_SET);
        Motor_Left_Move(PID_Speed_Lout);
//        osDelay(1);
        Motor_Right_Move(PID_Speed_Rout);
//        HAL_GPIO_WritePin(TEST_GPIO_Port,TEST_Pin,GPIO_PIN_RESET);
//        osDelay(1);
        osDelayUntil(&Contiol_time,2);
    }

}


void Motor_Left_Move(int duty)
{
    if (duty>=0)
    {
        __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, duty);
        __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 0);
    } else
    {
        __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 0);
        __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, -duty);
    }

}

void Motor_Right_Move(int duty)
{
    if (duty>=0)
    {
        __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, duty);
        __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, 0);
    } else
    {
        __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3,0);
        __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, -duty);
    }
}
//TODO: 电机速度计算  看看能不能继续提高速度和扭矩